#!/usr/bin/env python
#
# Node receives sailing_state from heading_control
# Outputs tack_sail and tack_rudder to override normal direction control

import rospy
from std_msgs.msg import Float32
from std_msgs.msg import String

from sailing_robot.tack_control import Tacking

if __name__ == '__main__':
    try:
        sail_pub = rospy.Publisher('tack_sail', Float32, queue_size=10)
        rudder_pub = rospy.Publisher('tack_rudder', Float32, queue_size=10)
        tacking = Tacking()
        def recv(msg):
            sail, rudder = tacking.calculate_sail_and_rudder(msg.data)
            sail_pub.publish(sail)
            rudder_pub.publish(rudder)

        rospy.init_node("publish_tack_data", anonymous=True)
        rospy.Subscriber('sailing_state', String, recv)
        rospy.spin()  # Wait for shutdown
    except rospy.ROSInterruptException:
        pass
